#pragma onec;

#include <bits/stdc++.h>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/SVD>
#include <eigen3/Eigen/Dense>  
  
//using Eigen::MatrixXf;  
using namespace Eigen;  
using namespace Eigen::internal;  
using namespace Eigen::Architecture;  


// 定义传感器类型
class sensor
{

public:
    using EV6 = Eigen::Matrix<double, 6, 1>;
    sensor(/* args */){}
    virtual ~sensor(){}
    virtual EV6 measure(Eigen::Matrix3d pos, Eigen::Vector3d trans){

    }
};


// 定义激光传感器
class laser: public sensor{
public:
    laser(){

    }
    ~laser(){

    }
    // 赋值世界坐标系里面的栏杆
    std::vector<Eigen::Vector3d> mWalls;//简单栏杆存储 
    void setWall(std::vector<Eigen::Vector3d> _Walls){// 设置栏杆的参数
        mWalls = _Walls;// copy 墙面的信息
    }
    // 给定一位姿 求解出来 激光所探测出来的距离  返回计算的在世界坐标系的点  在laser坐标系的点
    virtual EV6 measure(Eigen::Matrix3d pos, Eigen::Vector3d trans){
        if(mWalls.size() == 0){
            std::cout << "Wall Num Is Zero" << std::endl;
        }   
        // 激光器在激光坐标系是 d 0 0的形式   
        // step 1把两个点转到世界坐标系下面
        Eigen::Vector3d p0 = trans;
        p0[2] = 1;// 转换成齐次的
        Eigen::Vector3d p1 = pos * Eigen::Vector3d(1., 0., 0.) + trans;
        p1[2] = 1;// 转换成齐次的
        // step 2求出两个点计算出一条直线参数 
        // 两个点计算成直线参数 只计算二维度的
        Eigen::Matrix<double, 2, 3> H;
        H << p0[0], p0[1], 1., p1[0], p1[1], 1.;
        Eigen::Matrix<double, 3, 3> HTH = H.transpose() * H;// H矩阵
        JacobiSVD<Eigen::MatrixXd> svd(HTH, ComputeThinU | ComputeThinV );
        Eigen::Matrix3d V = svd.matrixV();
        // 拿到右奇异向量
        Eigen::Vector3d A = V.block<3, 1>(0, 2);
        // debug 验证Ax  debug ok
        // std::cout << "A = " << A.transpose() << std::endl;
        // std::cout << "HTH*A = " << (HTH * A).transpose() << std::endl;
        // std::cout << "A*p1 = " << A.transpose() * p0 << std::endl;
        // std::cout << "A*p2 = " << A.transpose() * p1 << std::endl;

        // step 3遍历所有找相交的地方
        std::vector<Eigen::Vector3d> intersections;
        for(auto & wall : mWalls){
            Eigen::Vector3d res = wall.cross(A);
            if(abs(res[2]) < 1e-5) continue;// 此时是平行的情况
            res = res / res[2];// 把最后一个维度归一化到1去
            // debug ok
            // std::cout << wall.transpose() << " × " <<  A.transpose() << " = " << res.transpose() << std::endl;
            res[2] = 0;// 恢复成3d点 是在世界坐标系下的3d点
            intersections.push_back(res);// 此时是3d点     
        }
        // step 4转换回激光器坐标系 保留最小的且是正的返回
        Eigen::Matrix3d invR = pos.transpose();
        Eigen::Vector3d invT = - invR * trans;
        EV6 res;
        res << -1.,-1.,-1.,-1.,-1.,-1.;// 刚开始都设置成-1
        for(size_t i = 0; i < intersections.size(); ++i){
            auto & point = intersections[i];
            Eigen::Vector3d pInLaser = invR * point + invT;
            if(pInLaser[0] > 0){// 判断是x方向的了
                if(res(3, 0) < 0){
                    res.block<3,1>(0,0) = intersections[i];
                    res.block<3,1>(3,0) = pInLaser;
                }else{
                    if(res(3, 0) > pInLaser[0]){
                        res.block<3,1>(0,0) = intersections[i];
                        res.block<3,1>(3,0) = pInLaser;
                    }
                }
            } 
        }
        return res;

    }
};

